function [pL3S1,pL3S2,pL3S3]=servoL3(AngShoulder,AngLong,AngFoot)
    S1=pi-AngShoulder+pi/2;
    S2=pi+AngLong;
    S3=-AngFoot+pi/2;
    %计算单腿三个舵机角度
    pL3S1=round(S1*2000/pi+500);
    if (pL3S1<500)
        pL3S1=500;
    elseif (pL3S1>2500)
        pL3S1=2500;
    end
    %
    pL3S2=round(S2*2000/pi+500);
    if (pL3S2<500)
        pL3S2=500;
    elseif (pL3S2>2500)
            pL3S2=2500;
    end
    %
    pL3S3=round(S3*2000/pi+500);
    if (pL3S3<500)
        pL3S3=500;
    elseif (pL3S3>2500)
            pL3S3=2500;
    end
end